#include "sv2_camera_base.h"


#define SV2_MAX_FRAMES 52000
typedef unsigned char byte;


namespace sv2 {
Camera::Camera(CameraType type, int id)
{
  this->_is_running = false;
  this->_is_updated = false;
  this->_type = type;
  
  this->_width = -1;
  this->_height = -1;
  this->_fps = -1;
  this->_ip = "192.168.2.64";
  this->_port = -1;
  this->_brightness = -1;
  this->_contrast = -1;
  this->_saturation = -1;
  this->_hue = -1;
  this->_exposure = -1;
  this->_fourcc = "MJPG";

  this->open(type, id);
}
Camera::~Camera()
{
  this->_is_running = false;
  // this->_tt.join();
}
std::string Camera::getFourcc()
{
  return this->_fourcc;
}
void Camera::setFourcc(std::string fourcc)
{
  this->_fourcc = fourcc;
}
void Camera::setWH(int width, int height)
{
  this->_width = width;
  this->_height = height;
}
void Camera::setFps(int fps)
{
  this->_fps = fps;
}
void Camera::setIp(std::string ip)
{
  this->_ip = ip;
}
void Camera::setRtspUrl(std::string rtsp_url)
{
  this->_rtsp_url = rtsp_url;
}
void Camera::setVideoPath(std::string video_path)
{
  this->_video_path = video_path;
}
void Camera::setPort(int port)
{
  this->_port = port;
}
void Camera::setBrightness(double brightness)
{
  this->_brightness = brightness;
}
void Camera::setContrast(double contrast)
{
  this->_contrast = contrast;
}
void Camera::setSaturation(double saturation)
{
  this->_saturation = saturation;
}
void Camera::setHue(double hue)
{
  this->_hue = hue;
}
void Camera::setExposure(double exposure)
{
  this->_exposure = exposure;
}

int Camera::getW()
{
  return this->_width;
}
int Camera::getH()
{
  return this->_height;
}
int Camera::getFps()
{
  return this->_fps;
}
std::string Camera::getIp()
{
  return this->_ip;
}
int Camera::getPort()
{
  return this->_port;
}
double Camera::getBrightness()
{
  return this->_brightness;
}
double Camera::getContrast()
{
  return this->_contrast;
}
double Camera::getSaturation()
{
  return this->_saturation;
}
double Camera::getHue()
{
  return this->_hue;
}
double Camera::getExposure()
{
  return this->_exposure;
}
bool Camera::isRunning()
{
  return this->_is_running;
}

void Camera::openImpl()
{
  if (this->_type == CameraType::WEBCAM || this->_type == CameraType::V4L2CAM)
  {
    if (this->_type == CameraType::V4L2CAM)
    {
      this->_cap.open(this->_camera_id, cv::CAP_V4L2);
    }
    if (this->_type == CameraType::WEBCAM)
    {
      this->_cap.open(this->_camera_id);
    }
    if (_fourcc.size() >= 4)
    {
      const char *fourcc_cstr = _fourcc.c_str();
      this->_cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc(fourcc_cstr[0], fourcc_cstr[1], fourcc_cstr[2], fourcc_cstr[3]));
    }
    if (this->_width > 0 && this->_height > 0)
    {
      this->_cap.set(cv::CAP_PROP_FRAME_WIDTH, this->_width);
      this->_cap.set(cv::CAP_PROP_FRAME_HEIGHT, this->_height);
    }
    if (this->_fps > 0)
    {
      this->_cap.set(cv::CAP_PROP_FPS, this->_fps);
    }
    if (this->_brightness > 0)
    {
      this->_cap.set(cv::CAP_PROP_BRIGHTNESS, this->_brightness);
    }
    if (this->_contrast > 0)
    {
      this->_cap.set(cv::CAP_PROP_CONTRAST, this->_contrast);
    }
    if (this->_saturation > 0)
    {
      this->_cap.set(cv::CAP_PROP_SATURATION, this->_saturation);
    }
    if (this->_hue > 0)
    {
      this->_cap.set(cv::CAP_PROP_HUE, this->_hue);
    }
    if (this->_exposure > 0)
    {
      this->_cap.set(cv::CAP_PROP_EXPOSURE, this->_exposure);
    }
  }
  else if (this->_type == CameraType::G1)
  {
    char pipe[512];
    if (this->_width <= 0 || this->_height <= 0)
    {
      this->_width = 1280;
      this->_height = 720;
    }
    if (this->_port <= 0)
    {
      this->_port = 554;
    }
    if (this->_fps <= 0)
    {
      this->_fps = 30;
    }

    sprintf(pipe, "rtspsrc location=rtsp://%s:%d/H264?W=%d&H=%d&FPS=%d&BR=4000000 latency=0 ! \
                   application/x-rtp,media=video ! rtph264depay ! parsebin ! mppvideodec ! \
                   videoconvert ! appsink drop=true", 
                   this->_ip.c_str(), this->_port, this->_width, this->_height, this->_fps);

    // printf("%s\r\n",pipe);
    this->_cap.open(pipe, cv::CAP_GSTREAMER);
  }
  else if (this->_type == CameraType::GX40)
  {
    std::ostringstream camera_url;
    if (this->_width <= 0 || this->_height <= 0)
    {
      this->_width = 1280;
      this->_height = 720;
    }
    if (this->_port <= 0)
    {
      this->_port = 554;
    }
#if defined(PLATFORM_X86_CUDA) || defined(PLATFORM_X86_INTEL)
    camera_url << "rtspsrc location = rtsp://user:0000@" << this->_ip << ":" << this->_port
               << "/cam/realmonitor?channel=1&subtype=0 latency=100 ! application/x-rtp,media=video ! \
               rtph265depay ! parsebin ! avdec_h265 ! videoscale ! video/x-raw,width=(int)" << this->_width
               << ",height=(int)" << this->_height << " ! videoflip video-direction=4 ! videoconvert ! video/x-raw,format=(string)BGR ! \
               appsink sync=false";
      this->_cap.open(camera_url.str(), cv::CAP_GSTREAMER);
#endif
#ifdef PLATFORM_JETSON
    camera_url << "rtspsrc location = rtsp://user:0000@" << this->_ip << ":" << this->_port
               << "/cam/realmonitor?channel=1&subtype=0 latency=100 ! \
               application/x-rtp,media=video ! rtph265depay ! parsebin ! \
               nvv4l2decoder ! nvvidconv ! \
               video/x-raw,format=(string)BGRx,width=(int)"
               << this->_width << ",height=(int)" << this->_height << " ! videoconvert ! video/x-raw,format=(string)BGR ! \
               appsink sync=false";
    this->_cap.open(camera_url.str(), cv::CAP_GSTREAMER);
#endif
  }
  else if (this->_type == CameraType::SU17)
  {
    char pipe[512];
    if (this->_width <= 0 || this->_height <= 0)
    {
      this->_width = 1280;
      this->_height = 720;
    }
    if (this->_port <= 0)
    {
      this->_port = 1234;
    }
    if (this->_fps <= 0)
    {
      this->_fps = 25;
    }

#if defined(PLATFORM_X86_INTEL)
    sprintf(pipe, "rtspsrc location=rtsp://%s:1234/test.sdp latency=100 ! \
                   application/x-rtp,media=video ! rtph264depay ! parsebin ! vaapih264dec ! \
                   vaapipostproc ! video/x-raw,format=YV12, width=(int)%d, height=(int)%d,framerate=25/1 ! videoconvert ! videorate ! video/x-raw,framerate=25/1 ! \
                   appsink sync=false",
                   this->_ip.c_str(), this->_width, this->_height, this->_fps);
    this->_cap.open(pipe, cv::CAP_GSTREAMER);
#endif
  }
  else if (this->_type == CameraType::MIPI)
  {
    char pipe[512];
    if (this->_width <= 0 || this->_height <= 0)
    {
      this->_width = 1280;
      this->_height = 720;
    }
    if (this->_fps <= 0)
    {
      this->_fps = 30;
    }
#if defined(PLATFORM_JETSON)
    sprintf(pipe, "nvarguscamerasrc sensor-id=%d ee-mode=0 tnr-mode=0 aeantibanding=0 wbmode=0 ! \
                   video/x-raw(memory:NVMM), width=(int)%d, height=(int)%d, format=(string)NV12, framerate=(fraction)%d/1 ! \
                   nvvidconv ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink",
                   this->_camera_id, this->_width, this->_height, this->_fps);
#endif
#if defined(PLATFORM_ROCKCHIP)
    sprintf(pipe, "v4l2src device=/dev/video%d io-mode=4 ! video/x-raw, format=NV12, width=(int)%d, height=(int)%d, framerate=%d/1 ! videoconvert ! video/x-raw, format=(string)BGR ! appsink",
                   this->_camera_id, this->_width, this->_height, (int)this->_fps);
    std::cout << pipe << std::endl;
#endif
    this->_cap.open(pipe, cv::CAP_GSTREAMER);
  }
  else if (this->_type == CameraType::RTSP)
  {
    char pipe[512];
    if (this->_width <= 0 || this->_height <= 0)
    {
      this->_width = 1280;
      this->_height = 720;
    }
    if (this->_port <= 0)
    {
      this->_port = 554;
    }
    if (this->_fps <= 0)
    {
      this->_fps = 30;
    }

#if defined(PLATFORM_X86_CUDA)
    sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! \
                   application/x-rtp,media=video ! rtph264depay ! parsebin ! avdec_h264 ! \
                   videoconvert ! appsink sync=false", 
                   this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps);
    this->_cap.open(pipe, cv::CAP_GSTREAMER);
#endif
#if defined(PLATFORM_X86_INTEL)
    sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! \
                   application/x-rtp,media=video ! rtph264depay ! parsebin ! vaapih264dec ! \
                   vaapipostproc ! video/x-raw,format=YV12,framerate=25/1 ! videoconvert ! videorate ! video/x-raw,framerate=25/1 ! \
                   appsink sync=false",
                   this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps);
    this->_cap.open(pipe, cv::CAP_GSTREAMER);
#endif
#ifdef PLATFORM_JETSON
    sprintf(pipe, "rtspsrc location=%s?W=%d&H=%d&FPS=%d latency=100 ! application/x-rtp,media=video ! rtph264depay ! parsebin ! nvv4l2decoder enable-max-performancegst=1 ! nvvidconv ! video/x-raw,format=(string)BGRx ! videoconvert ! appsink sync=false", this->_rtsp_url.c_str(), this->_width, this->_height, this->_fps);
    this->_cap.open(pipe, cv::CAP_GSTREAMER);
#endif
  }
  else if (this->_type == CameraType::VIDEO)
  {
    this->_cap.open(this->_video_path);
  }
}
void Camera::open(CameraType type, int id)
{
  this->release();
  this->_type = type;
  this->_camera_id = id;

  openImpl();

  if (this->_cap.isOpened())
  {
    std::cout << "Camera opened!" << std::endl;
    this->_is_running = true;
    this->_tt = std::thread(&Camera::_run, this);
    this->_tt.detach();
  }
  else if (type != CameraType::NONE)
  {
    std::cout << "Camera can NOT open!" << std::endl;
  }
}
void Camera::_run()
{
  while (this->_is_running && this->_cap.isOpened())
  {
    if (this->_type != CameraType::VIDEO || this->_is_updated == false)
    {
      this->_cap >> this->_frame;
      this->_is_updated = true;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(2));
  }
}
bool Camera::read(cv::Mat& image)
{
  if (this->_type != CameraType::NONE)
  {
    int n_try = 0;
    while (n_try < 5000)
    {
      if (this->_is_updated)
      {
        this->_frame.copyTo(image);
        this->_is_updated = false;
        break;
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(2));
      n_try ++;
    }
  }
  if (image.cols == 0 || image.rows == 0)
  {
    throw std::runtime_error("SpireCV 2.0 (101) Camera cannot OPEN, please check CAMERA_ID or CAMERA_IP!");
  }
  return image.cols > 0 && image.rows > 0;
}
void Camera::release()
{
  this->_is_running = false;
  this->_is_updated = false;
  if (this->_cap.isOpened())
    this->_cap.release();
}

}

